import cv2
import numpy as np
from matplotlib import pyplot as plt

img1 = cv2.imread('../misc_pic/apt1.jpg',0)  #queryimage # left image
img2 = cv2.imread('../misc_pic/apt2.jpg',0) #trainimage # right image

surf=cv2.xfeatures2d.SURF_create(4000)

# find the keypoints and descriptors with SIFT
kp1, des1 = surf.detectAndCompute(img1,None)
kp2, des2 = surf.detectAndCompute(img2,None)

# FLANN parameters
FLANN_INDEX_KDTREE = 1
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50)

flann = cv2.FlannBasedMatcher(index_params,search_params)
#matches = flann.match(des1,des2)
matches=flann.knnMatch(des1,des2,k=2)

good=[]; pts1=[]; pts2=[]
for (m,n) in matches:
    if m.distance < 0.7*n.distance:
        good.append(m)
        pts2.append(kp2[m.trainIdx].pt)
        pts1.append(kp1[m.queryIdx].pt)

"""
matches = sorted(good,key=lambda x:x.distance) #按距离排序
pts1.clear(); pts2.clear()
for x in matches:
    pts1.append(kp1[x.queryIdx].pt)
    pts2.append(kp2[x.trainIdx].pt)
"""

#img3=cv2.drawMatches(img1,kp1,img2,kp2,good,img2,flags=2)
#cv2.imwrite("e:\out.jpg",img3)

pts1 = np.int32(pts1)
pts2 = np.int32(pts2)

N=100
e1a=np.zeros(N)
e1b=np.zeros(N)
e2a=np.zeros(N)
e2b=np.zeros(N)

for i in range(N):
    ##对pts1,pts2乱序
    N=len(pts1)
    all_idxs = np.arange(N)
    np.random.shuffle(all_idxs)
    np.random.shuffle(all_idxs)
    tpts1=pts1[all_idxs]
    tpts2=pts2[all_idxs]

    Fs, mask = cv2.findFundamentalMat(tpts1,tpts2,cv2.FM_RANSAC,1,0.99)
    F=Fs[:3,:]
    #print(np.linalg.det(F))
    #print(F)

    ##计算极点，e1,e2(e1为相机2中心C在img1中的坐标，e2为相机1中心C在img2中的坐标)
    Ft=F.transpose()
    U,s,V=np.linalg.svd(F)
    e1=V[-1,:]; e1=e1/e1[2]
    U,s,V=np.linalg.svd(Ft)
    e2=V[-1,:]; e2=e2/e2[2]
    print(i)
    print(e1)
    print(e2)
    e1a[i]=e1[0]
    e1b[i]=e1[1]
    e2a[i]=e2[0]
    e2b[i]=e2[1]

plt.subplot(411)
plt.plot(e1a)
plt.subplot(412)
plt.plot(e1b)
plt.subplot(413)
plt.plot(e2a)
plt.subplot(414)
plt.plot(e2b)
plt.show()


"""
##显示img1中点在img2中的极线,l=Fx
#先显示img2
width,height=img2.shape[:2]
t=np.linspace(0,height,100)
plt.gray()
plt.imshow(img2)
for kp in pts1[:8]:
    x=[kp[0],kp[1],1]
    line=np.dot(F,x)
    #线上的点
    lt = np.array([(line[2]+line[0]*tt)/(-line[1]) for tt in t])
    ndx = (lt>=0) & (lt<width)  ## 仅取在图像范围内的点
    plt.plot(t[ndx],lt[ndx],linewidth=2)
plt.show()

"""